//
// Created by cch on 2022/5/1.
//

#include "imu_gps_fusion.h"

IMUGPS::Fusion::Fusion(const double acc_noise,const double gyro_nosie,
                       const double acc_bias_noise,const double gyro_bias_noise,
                       const Eigen::Vector3d &I_P_GPS)
{
   initializer = std::make_unique<Initializer>(I_P_GPS);
   imuProcess = std::make_unique<ImuProcess>(acc_noise,gyro_nosie,acc_bias_noise,gyro_bias_noise,Eigen::Vector3d(0.,0.,-9.817));
   gpsProcesser = std::make_unique<GpsProcesser>(I_P_GPS);
}

void IMUGPS::Fusion::ProcessGpsData(const GpsDataPtr &gpsDataPtr) {
  initializer->AddGpsData(gpsDataPtr,state_);
  init_lla = gpsDataPtr->lla;

  gpsProcesser->Update(init_lla,gpsDataPtr,state_);

}

bool IMUGPS::Fusion::ProcessImuData(const ImuDataPtr &imuDataPtr,State * fused_state) {

    initializer->AddImuData(imuDataPtr);
    imuProcess->Predict(imuDataPtr,imuDataPtr,state_);

    fused_state = state_;
    return true;
}